#include <cmath>
#include <string>

#include <QTextStream>
#include <QDebug>

#include "poly3.h"

static double dStep = 1.0;  //设置取样点的步长距离
static double dDataPointX = 0;  //数据点x坐标
static double dDataPointY = 0;  //数据点y坐标
static double dDataPointHdg = 0;  //数据点切角hdg


Poly3::Poly3()
{
}

Poly3::~Poly3()
{
}

void Poly3::testPoly3(PointHdg geometryPoly3)
{
    QTextStream out(stdout);

    std::string strX, strY, strHdg, strS;
    for(int i = 0; i < geometryPoly3.qvecPointData.size(); ++i)
    {
        //输出测试
        strX = std::to_string(geometryPoly3.qvecPointData[i].rx());
        strY = std::to_string(geometryPoly3.qvecPointData[i].ry());
        strHdg = std::to_string(geometryPoly3.qvecHdgData[i]);
        strS = std::to_string(geometryPoly3.qvecSData[i]);
        qDebug() << strX.c_str() << " " << strY.c_str() << " " << strHdg.c_str() << " " << strS.c_str();
    }

    out << "xxxxxxxxxx" << endl << "[";
    for(int i = 0; i < geometryPoly3.qvecPointData.size(); ++i)
    {
        out << geometryPoly3.qvecPointData[i].rx() << ",";
    }

    out << "]" << endl << "yyyyyyyyyyy" << endl << "[";
    for(int i = 0; i < geometryPoly3.qvecPointData.size(); ++i)
    {
        out << geometryPoly3.qvecPointData[i].ry() << ",";
    }
    out << "]" << endl;
}

//根据参数x,y,hdg,a,b,c,d确定该三次多项式,根据不同位置的数据点描述曲线(目前自变量u的范围只能由s_start取到dSEnd,取值范围不对,但是因为曲线变化缓慢,类似直线,存在很小的误差)
PointHdg Poly3::geometryPoly3Data(double dXCoordinate, double dYCoordinate, double dHdg, double dA, double dB, double dC, double dD, double dSStart, double dSEnd)
{
    PointHdg geometryPoly3;
    QVector<QPointF> qvecGeometryPoly3;
    QVector<double> qvecHdg;
    QVector<double> qvecS;
    QPointF qpointf;
    double dU;  //自变量u

    for(double s = dSStart; s < dSEnd; s += dStep)
    {
        //考虑将数据点的插入向量也封装成函数********************这样和下面最后一个点的插入再次统一起来
        dU = s;
        subFunctionPoly3(dXCoordinate, dYCoordinate, dHdg, dA, dB, dC, dD, dU);

        qpointf.setX(dDataPointX);
        qpointf.setY(dDataPointY);

        qvecGeometryPoly3.push_back(qpointf);
        qvecHdg.push_back(dDataPointHdg);
        qvecS.push_back(s);
    }

    dU = dSEnd;
    subFunctionPoly3(dXCoordinate, dYCoordinate, dHdg, dA, dB, dC, dD, dU);

    qpointf.setX(dDataPointX);
    qpointf.setY(dDataPointY);

    qvecGeometryPoly3.push_back(qpointf);
    qvecHdg.push_back(dDataPointHdg);
    qvecS.push_back(dSEnd);

    geometryPoly3.qvecPointData = qvecGeometryPoly3;
    geometryPoly3.qvecHdgData = qvecHdg;
    geometryPoly3.qvecSData = qvecS;

    return geometryPoly3;
}

//三次多项式子函数
void Poly3::subFunctionPoly3(double dXCoordinate, double dYCoordinate, double dHdg, double dA, double dB, double dC, double dD, double dU)
{
    double dSTSpinAngle = atan(dB);  //ST坐标系旋转角度
    double dV;  //因变量v
    double dUVTan = 0;  //(u,v)点在UV坐标系的正切值
    double dUVAngle;  //(u,v)点在UV坐标系的角度
    double dUVSpinAngle = dHdg - dSTSpinAngle;  //UV坐标系旋转
    double dUVDistance;  //(u,v)点在UV坐标系的距离
    double dUVHdg;  //(u,v)点在UV坐标系的切角
    dV = dA + dB * dU + dC * pow(dU,2) + dD * pow(dU,3);

    if(0 != dU)
    {
        dUVTan = tan(dV / dU);
    }

    dUVAngle = atan(dUVTan);

    dUVDistance = sqrt(pow(dU,2) + pow(dV,2));
    dDataPointX = dUVDistance * cos(dUVSpinAngle + dUVAngle) + dXCoordinate + dA * sin(dUVSpinAngle);
    dDataPointY = dUVDistance * sin(dUVSpinAngle + dUVAngle) + dYCoordinate - dA * cos(dUVSpinAngle);
    dUVHdg = atan(dB + 2 * dC * dU + 3 * dD * pow(dU,2));
    dDataPointHdg = dUVHdg + dUVSpinAngle;
}
